package legoLab04;

import lejos.nxt.*;

public class Sejway {

	// PID constants
	int KP = 150;
	int KI = 4;
	int KD = 15;
	int SCALE = 33;
	
	//Strings
	final String ER = "error ";
	final String IE = "int_error ";
	final String DE = "deriv_error ";
	final String PV = "pid_value ";
	final String FR = "RAM: ";
	
	// Global vars:
	int offset;
	int prev_error;
	float int_error;
	
	LightSensor ls;
	
	public Sejway() {
		ls = new LightSensor(SensorPort.S2, true);
		Motor.B.regulateSpeed(false);
		Motor.C.regulateSpeed(false);
	}
	
	public void getBalancePos() {
		// Wait for user to balance and press orange button
		while (!Button.ENTER.isPressed()) {
			// NXTway must be balanced.
			offset = 494;
//			offset = ls.readNormalizedValue();
			LCD.clear();
			LCD.drawInt(offset, 5, 5);
			LCD.refresh();
		}
		LCD.clear();
	}
	
	public void pidControl() throws InterruptedException {
		LCD.clear();
		int normVal, error, deriv_error, KP_Error, KI_int_error, KD_deriv_error, pid_val, power;
		String empty = "                   ";
		
		//add offset control
		Button.LEFT.addButtonListener(new ButtonListener() {
			
			@Override
			public void buttonReleased(Button arg0) {
				offset-=2;
			}
			
			@Override
			public void buttonPressed(Button arg0) {}
		});
		
		Button.RIGHT.addButtonListener(new ButtonListener() {
			
			@Override
			public void buttonReleased(Button arg0) {
				offset+=2;
			}
			
			@Override
			public void buttonPressed(Button arg0) {}
		});
		
		while (!Button.ESCAPE.isPressed()) {
			normVal = ls.readNormalizedValue();

			// Proportional Error:
			error = normVal - offset;
			// Adjust far and near light readings:
			if (error < 0) error = (int)(error * 1.8F);
//			else error = (int)(error / 10);
			
			
			// Integral Error:
			int_error = ((int_error + error) * 2)/3;
			
			// Derivative Error:
			deriv_error = error - prev_error;
			prev_error = error;
			
			KP_Error = KP*error;
			KI_int_error = (int)(KI * int_error);
			KD_deriv_error = KD * deriv_error;
			pid_val = (int)(KP_Error + KI_int_error + KD_deriv_error) / SCALE;
//			pid_val = (int)(KP * error + KI * int_error + KD * deriv_error) / SCALE;

			
			if (pid_val > 100)
				pid_val = 100;
			if (pid_val < -100)
				pid_val = -100;

			// Power derived from PID value:
			power = Math.abs(pid_val);
			power = 55 + (power * 65) / 100; // NORMALIZE POWER
			Motor.B.setPower(power);
			Motor.C.setPower(power);

			if (pid_val > 0) {
				Motor.B.forward();
				Motor.C.forward();
			} else {
				Motor.B.backward();
				Motor.C.backward();
			}
			
			for(int i = 0; i <= 7; i++){
				LCD.drawString(empty, 0, i);
			}
			LCD.refresh();
			
			LCD.drawInt(KP_Error, 0, 1);
			LCD.drawInt(KI_int_error, 0, 2);
//			LCD.drawInt(KD_deriv_error, 0, 3);
			LCD.drawInt(pid_val, 0, 4);
//			LCD.drawInt((int) Runtime.getRuntime().freeMemory(), 0, 5);
			LCD.drawInt(offset, 0, 6);
			LCD.drawInt(normVal, 0, 7);
			
			LCD.refresh();
			Thread.sleep(2);
		}
	}
	
	public void shutDown() {
		// Shut down light sensor, motors
		Motor.B.flt();
		Motor.C.flt();
		ls.setFloodlight(false);
	}
	
	public static void main(String[] args) throws InterruptedException {
		Sejway sej = new Sejway();
		sej.getBalancePos();
		sej.pidControl();
		sej.shutDown();
	}
}